# Untitled - By: YANCL - 周日 7月 23 2023

import sensor, image, time
import lcd              #LCD显示屏幕
import json
from pyb import UART    #串口

sensor.reset()                          # Reset and initialize the sensor.
sensor.set_pixformat(sensor.RGB565)  # Set pixel format to RGB565 (or GRAYSCALE)
sensor.set_framesize(sensor.QQVGA2)     # Set frame size to QQVGA2 (128x160)
#sensor.set_windowing((240, 240))       # Set 240x240 window.
sensor.skip_frames(time=2000)           # Let the camera adjust.
lcd.init() # Initialize the lcd screen.
uart = UART(3, 115200)

Rectangle_ROI1 = (0,  5, 128, 16)
Rectangle_ROI2 = (0, 25, 128, 16)
Rectangle_ROI3 = (0, 45, 128, 16)
Rectangle_ROI4 = (0, 65, 128, 16)
red_thresholds = (47, 0, 78, 11, 65, -31)   #红色阈值

#redline_rec1 = 0
#redline_rec2 = 0
#redline_rec3 = 0
#redline_rec4 = 0

clock = time.clock()


while(True):
    clock.tick()
    img = sensor.snapshot()
    #img.draw_line((64, 0, 64, 160), color = 255)        #划屏幕中线

    #画四个框
    #img.draw_rectangle(Rectangle_ROI1, color = (255,255,255))
    #img.draw_rectangle(Rectangle_ROI2, color = (255,255,255))
    #img.draw_rectangle(Rectangle_ROI3, color = (255,255,255))
    #img.draw_rectangle(Rectangle_ROI4, color = (255,255,255))



    #识别框中 红色色块
    red_bolb1 = img.find_blobs([red_thresholds], roi = Rectangle_ROI1)  #识别红色色块，在ROI1
    red_bolb2 = img.find_blobs([red_thresholds], roi = Rectangle_ROI2)
    red_bolb3 = img.find_blobs([red_thresholds], roi = Rectangle_ROI3)
    red_bolb4 = img.find_blobs([red_thresholds], roi = Rectangle_ROI4)

    if len(red_bolb1) == 1:
        # Draw a rect around the blob.
        redline_rec1 = red_bolb1[0]   #红色矩形色块1，在第1个框内的矩形
        img.draw_rectangle(redline_rec1[0:4]) # rect
        img.draw_cross(redline_rec1[5], redline_rec1[6]) # cx, cy
    if len(red_bolb2) == 1:
        # Draw a rect around the blob.
        redline_rec2 = red_bolb2[0]   #红色矩形色块2，在第2个框内的矩形
        img.draw_rectangle(redline_rec2[0:4]) # rect
        img.draw_cross(redline_rec2[5], redline_rec2[6]) # cx, cy
    if len(red_bolb3) == 1:
        # Draw a rect around the blob.
        redline_rec3 = red_bolb3[0]   #红色矩形色块2，在第2个框内的矩形
        img.draw_rectangle(redline_rec3[0:4]) # rect
        img.draw_cross(redline_rec3[5], redline_rec3[6]) # cx, cy
    if len(red_bolb4) == 1:
        # Draw a rect around the blob.
        redline_rec4 = red_bolb4[0]   #红色矩形色块2，在第2个框内的矩形
        img.draw_rectangle(redline_rec4[0:4]) # rect
        img.draw_cross(redline_rec4[5], redline_rec4[6]) # cx, cy
    lcd.display(img) # Take a picture and display the image.
    #计算距离偏差 没有取平均值(浮点数)，直接发送偏差之和
    Distance_Error = (int)((redline_rec1[5]-64) + (redline_rec2[5]-64) + (redline_rec3[5]-64)+ (redline_rec4[5]-64))
    #if (len(red_bolb1) == 0 and len(red_bolb2) == 0 and len(red_bolb3) == 0 and len(red_bolb4) == 0):
        #Distance_Error = 0
    print(Distance_Error)

    FH = bytearray([0xAA, 0x00])  #包头
    uart.write(FH)          #发送包头
    data = bytearray([Distance_Error, 0x00])
    uart.write(data)        #发送距离误差的数据
    uart.write(FH)          #发送包尾


    #uart.write("Hello World!\r")
    #uart.write(Distance_Error)

    #print(red_bolb1[5])
    #print(clock.fps())
